package com.it1997.astar;

import java.util.*;

public class Main {
    // 地图元素
    static final char START = 'S';  // 起点
    static final char END = 'E';  // 终点
    static final char SPACE = '.';  // 空地
    static final char WALL = 'W';  // 墙
    static final char VISITED = '-';  // 被访问过
    static final char ON_PATH = '@';  // 在结果路径上

    // 地图字符串
    static final String[] S_MAP = {
            ". . . . . . . . . . . . . . . . . . . .",
            ". . . W W W W . . . . . . . . . . . . .",
            ". . . . . . W . . . . . . . . . . . . .",
            ". . . . . . W . . . . . . . . . . . . .",
            ". . S . . . W . . . . . . . . . . . . .",
            ". . . . . . W . . . . . . . . . . . . .",
            ". . . . . . . . . . . . . . . . . . . .",
            ". . . . . . W . . . . . . . . . . . . .",
            ". . . W W W W . . . . . . . . . . . . .",
            ". . . W . . . . . . . . . . . . . E . ."
    };

    // 地图最大尺寸
    static Point MAX_PNT = new Point(S_MAP[0].replace(" ", "").length(), S_MAP.length);
    // 起点
    static Point START_PNT = null;
    // 终点
    static Point END_PNT = null;

    public static void main(String[] args) {

        char [][] cMap = new char[S_MAP[0].replace(" ", "").length()][S_MAP.length];
        genMap(cMap);
        printMap(cMap);

        search(cMap);

        printMap(cMap);
    }

    /**
     * 用地图字符串产生地图数据
     */
    static void genMap(char[][] cMap) {
        int idx = 0;
        for (String s : S_MAP) {
            char[] cs = s.replace(" ", "").toCharArray();
            for (int i = 0; i < cs.length; i++) {
                cMap[i][idx] = cs[i];
                switch (cs[i]) {
                    case START:
                        START_PNT = new Point(i, idx);
                        break;
                    case END:
                        END_PNT = new Point(i, idx);
                        break;
                }
            }
            idx++;
        }
    }

    /**
     * 打印地图
     */
    static void printMap(char [][] cMap) {
        for (int j = 0; j < MAX_PNT.y; j++) {
            for (int i = 0; i < MAX_PNT.x; i++) {
                System.out.printf("%c ", cMap[i][j]);
            }
            System.out.printf("\n");
        }
        System.out.printf("\n");
    }

    /**
     * 搜索算法
     */
    static void search(char [][] cMap) {
        Queue<Node> openList = new PriorityQueue<>(); // 优先队列(升序)
        List<Node> closList = new ArrayList<>();
        final int[][] directs = {{1, 0}, {0, 1}, {-1, 0}, {0, -1}}; // 可以扩展的四个方向
        openList.add(new Node(START_PNT, 0, 0, null)); // 把起始点放入堆
        Node lastNode = null; // 找到的最后一个点的数据,用来反推路径
        boolean finish = false;
        while (!finish && !openList.isEmpty()) {

            Node node = openList.poll(); // 取出f值最小的点
            final Point point = node.point;
            if (cMap[point.x][point.y] == SPACE) { // 将取出的点标识为已访问点
                cMap[point.x][point.y] = VISITED;
            }
            closList.add(node);
            for (int[] d : directs) { // 遍历四个方向的点
                final Point newPnt = new Point(point.x + d[0], point.y + d[1]);
                if (newPnt.x >= 0 && newPnt.x < MAX_PNT.x && newPnt.y >= 0 && newPnt.y < MAX_PNT.y) {
                    char e = cMap[newPnt.x][newPnt.y];
                    if (e == END) { // 如果是终点,则跳出循环,不用再找
                        lastNode = node;
                        finish = true;
                        break;
                    }

                    if (e != SPACE) { // 如果不是空地,就不需要再扩展
                        continue;
                    }

                    Node inQueueNode = findNodeInOpen(openList, newPnt);

                    if (inQueueNode != null) { // 如果在堆里,则更新g值
                        if (inQueueNode.g > node.g + 1) {
                            inQueueNode.g = node.g + 1;
                            inQueueNode.parent = node;
                        }
                    } else { // 如果不在堆里,则放入堆中
                        int h = h(newPnt);
                        Node newNode = new Node(newPnt, node.g + 1, h, node);
                        openList.add(newNode);
                    }
                }
            }
        }

        Stack<Node> stack= new Stack<>();
        // 反向找出路径
        for (Node pathNode = lastNode; pathNode != null; ) {
            Point pnt = pathNode.point;

            if (cMap[pnt.x][pnt.y] == VISITED) {
                cMap[pnt.x][pnt.y] = ON_PATH;
            }
            if (pathNode.parent != null) {
                stack.push(pathNode);
            }
            pathNode = pathNode.parent;
        }

        Node pop = stack.pop();
        System.out.println("x->"+pop.point.x);
        System.out.println("y->"+pop.point.y);
    }

    /**
     * 结点是否已经走过
     * @param inQueueNode
     * @return
     */
    private static boolean isExistClose(List <Node>closeList, Node inQueueNode) {
        if (closeList.isEmpty() || inQueueNode == null) {
            return false;
        }
        for (Node node : closeList) {
            if (inQueueNode.equals(node)) {
                return true;
            }
        }
        return false;
    }

    /**
     * h函数
     */
    static int h(Point pnt) {
//		return hBFS(pnt);
        return hManhattanDistance(pnt);
//		return hPowEuclidianDistance(pnt);
//		return hManhattanDistance(pnt);
    }

    /**
     * 曼哈顿距离,小于等于实际值
     */
    static int hManhattanDistance(Point pnt) {
        return Math.abs(pnt.x - END_PNT.x) + Math.abs(pnt.y - END_PNT.y);
    }

    /**
     * 欧式距离,小于等于实际值
     */
    static int hEuclidianDistance(Point pnt) {
        return (int) Math.sqrt(Math.pow(pnt.x - END_PNT.x, 2) + Math.pow(pnt.y - END_PNT.y, 2));
    }

    /**
     * 欧式距离平方,大于等于实际值
     */
    static double hPowEuclidianDistance(Point pnt) {
        return Math.pow(pnt.x - END_PNT.x, 2) + Math.pow(pnt.y - END_PNT.y, 2);
    }

    /**
     * BFS的h值,恒为0
     */
    static double hBFS(Point pnt) {
        return 0;
    }

    private static Node findNodeInOpen(Queue<Node> openList, Point point) {
        if (point == null || openList.isEmpty()) {
            return null;
        }
        for (Node n : openList) {
            if (point.equals(n.point)) {
                return n;
            }
        }
        return null;
    }

}
